#include <iostream>
#include <string>
#include <map>
#include "../video.hpp"
#include "../../../libpland/lib/plugin.hpp"
#include "../../../libpipc/lib/pipc.hpp"
#include <unistd.h>
#include <stdlib.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using namespace std;

using namespace cv;

using namespace pland;

class Motion : public UserC_Plugin
{
public:
    Motion();
    ~Motion();
    void start();
    void stop();
    void load_queues(SharedDQueue front ,SharedDQueue back);
    void setId(int id);
private:
    SharedDQueue front_;
    SharedDQueue back_;
    bool running_;
    int id_;
};


Motion::Motion()
{
    running_ = false;
}

Motion::~Motion()
{
}

void Motion::setId(int id)
{
    pipc::Pipc *set = pipc::Pipc::Instance();
    set->subscribe("motion.relarea",false);
    set->subscribe("motion.relarea" + utill_to_string(id),false);
    id_ = id;
}

void Motion::start()
{
    utill_log("Pluggin Motion Started ID:" + utill_to_string(id_));
    running_ = true;

    bool first_frame = true;
    cv::RNG rng(12345);
    cv::Mat img_prev;
    pipc::Pipc *set = pipc::Pipc::Instance();
    while(running_)
    {
        SharedData source = front_->pop();
        if(!isImgTag(source->get_header()->tag))
        {
            back_->push(source);
            continue;
        }
        try
        {
            cv::Mat img_bin, img_blured, img_diff;
            int rows = *((uint32_t*)(source->get_data()));
            int cols = (source->get_header()->size - sizeof(uint32_t))/rows;
            cv::Mat img_orig(rows, cols, CV_8U, ((char*)source->get_data()) + sizeof(uint32_t));

            cv::GaussianBlur(img_orig, img_blured, Size(0,0), 2, 2);

            if (first_frame)
            {
                img_prev  = img_blured.clone();
                first_frame = false;
                continue;
            }

            absdiff(img_blured, img_prev, img_diff);
            threshold(img_diff, img_bin, 30, 255, THRESH_BINARY);
            erode(img_bin,  img_bin, Mat(), Point(-1,-1), 3);
            dilate(img_bin, img_bin, Mat(), Point(-1,-1), 1);

            vector<vector<Point> > contours;
            vector<Vec4i> hierarchy;

            findContours( img_bin, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) );
            float area = 0;
            for(size_t i = 0; i< contours.size(); i++ )
            {
                area += contourArea(contours[i]);
                Scalar color = Scalar(rng.uniform(0,255));
                drawContours(img_orig, contours, i, color, 2, 8, hierarchy, 0, Point() );
            }

            pipc::Record r;
            r.data = utill_to_string(area);
            gettimeofday(&r.effective, NULL);
            if(source->get_header()->sequenceNumber % 5 == 0)
            {
                set->setValue("motion.relarea", r);
                set->setValue("motion.relarea" + utill_to_string(id_), r);
            }
            img_prev=img_blured.clone();
            back_->push(source);
        }
        catch(cv::Exception &e)
        {
            continue;
        }
    }
}

void Motion::stop()
{
    running_ = false;
}

void Motion::load_queues(SharedDQueue front ,SharedDQueue back)
{
    front_  = front;
    back_  = back;
}


extern "C"
{

UserC_Plugin* Core_GetInstance()
{
    return new Motion();
}

void Core_KillInstance(UserC_Plugin* a)
{
    delete dynamic_cast<Motion*>(a);
}

}
